## stock recruitment function
BH <- function(B, R0, h, phi0, err) {
  R <- 0.8*R0*h*B / (0.2*phi0*R0*(1-h) + (h-0.2)*B)
  as.matrix(R * err)
}

# still need varying lags
## base delay diff operating model
opmodel <- function(pars, m, T, E, L, phi0, errR = 0, errI = 0, errN = 0) {
  beta <- pars$beta        # transformed steepness
  mu <- pars$mu          # mean of transformed steepness
  tau <- pars$tau         # sd of transformed steepness
  B0 <- pars$B0          # unfished biomass
  p <- pars$p           # growth coefficient
  w <- pars$w           # w_k-1 / w_k
  M <- pars$M           # natural mortality
  sigR <- pars$sigR        # recruitment deviations
  qI <- pars$qI          # prop coeff for abundance
  sigI <- pars$sigI       # biomass observation errors
  sigN <- pars$sigN       # recruitment observation errors
  qN <- pars$qN         # prop coeff for recruitment
  q <- pars$q          # catchability?
  
  h <- (1+.2*exp(-beta)) / (1 + exp(-beta))
  # beta <- log((h - 0.2) / (1 - h))

  if (length(errR) + length(errI) + length(errN) == 3) {
    errR <- matrix(exp(rnorm((T+1)*m,0,sigR) - sigR^2 / 2), T+1, m)
    errI <- matrix(exp(rnorm((T+1)*m,0,sigI) - 0*sigI^2 / 2), T+1, m)
    errN <- matrix(exp(rnorm((T+1)*m,0,sigN) - 0*sigN^2 / 2), T+1, m)
  }
  
  R0 <- B0 / phi0
 # print(q)
  phi <- exp(-(E %*% diag(q, m, m))) # matrix(exp(-q*E), T+1, m) # percent non-catch
  
  B <- matrix(0, T+1, m)    # spawning stock biomass
  R <- matrix(0, T+1, m)    # recruitment
  C <- matrix(0, T+1, m)    # catch
  I <- matrix(0, T+1, m)    # biomass index
  N <- matrix(0, T+1, m)    # recruitment index
  
  # first year
  B[1,] <- B0
  C[1,] <- (1 - phi[1,])*B[1,]
  R[1,] <- BH(B[1,],R0,h,phi0,errR[1,]) # R0
  I[1,] <- qI*B[1,]*errI[1,] 
  N[1,] <- qN*R[1,]*errN[1,] 
  
  # second year  
  R[2,] <- BH(B[1,],R0,h,phi0,errR[2,]) # assume B_2-L = B_1-L 
  B[2,] <- exp(-M)*(1 + p - p*exp(-M))*(B[1,] - C[1,]) -
    p*w*exp(-M) * R[1,] * (B[1,] - C[1,])/B[1,] + R[2,]

  C[2,] <- (1 - phi[2,]) * B[2,]
  I[2,] <- qI * B[2,] * errI[2,]
  N[2,] <- qN * R[2,] * errN[2,]

  for (t in 2:T) {
    R[t+1,] <- BH(B[ifelse(t+1-L < 1, 1, t+1-L),],R0,h,phi0,errR[t+1,])
    B[t+1,] <- (1+p)*exp(-M)*(B[t,] - C[t,]) -
      p*exp(-2*M)*(B[t,] - C[t,]) / B[t,] * (B[t-1,] - C[t-1,]) -
        p*w*exp(-M)*(B[t,] - C[t,]) / B[t,] * R[t,] + R[t+1,]
    C[t+1,] <- (1 - phi[t+1,])*B[t+1,]
    I[t+1,] <- qI * B[t+1,] * errI[t+1,]
    N[t+1,] <- qN * R[t+1,] * errN[t+1,]
  }

  list(B = B[1:(T+1-L),], I = I[1:(T+1-L),], C = C[1:(T+1-L),],
       R = R[(L+1):(T+1),], N = N[1:(T+1-L),], E = E[1:(T+1-L),],
       h = h, beta = beta, phi = phi, T=T, L=L, qI = qI, qN = qN, m=m,
       q = q, B0 = B0, R0 = R0, phi0 = phi0, errR = errR, errI = errI, errN = errN)
}

## wrapper for running the operating model just for one stock
opmodel.sing <- function(k, pars, m, T, E, L, phi0, errR, errI, errN) {
  pars.k <- with(pars, list(beta=beta[k], mu=mu, tau=tau, B0=B0[k], p=p[k], w=w[k], M=M[k],
                            sigR=sigR, qI=qI, sigI=sigI, sigN=sigN, qN=qN, q=q[k]))
  m.k <- 1
  T.k <- T
  E.k <- t(t(E[,k]))
  L.k <- L # still needs changing
  phi0.k <- phi0[k]
  errR.k <- t(t(errR[,k]))
  errI.k <- t(t(errI[,k]))
  errN.k <- t(t(errN[,k]))
#  print(paste("sing", pars.k$q))
  opmodel(pars.k, m.k, T.k, E.k, L.k, phi0.k, errR.k, errI.k, errN.k)
}

## wrapper for finding the right q
opmodel.dep <- function(T_dep, q.dep, dep, k, pars, m, T, E, L, phi0,
                        errR, errI, errN) {
  pars$q[k] <- q.dep
#  print(paste("dep", q.dep))
  out <- opmodel.sing(k, pars, m, T, E, L, phi0, errR, errI, errN)
  (out$B[T_dep] - out$B0*dep)^2
}

## wrapper for finding perfect q
opmodel.pq <- function(q.dep, k, T_dep, dep, pars, m, T, E, L, phi0,
                       errR, errI, errN) {
  out <- opmodel.sing(k, pars, m, T, E, L, phi0, errR, errI, errN)
  (out$B[T_dep] - out$B0*dep)^2
}

## h -> beta
h_to_beta <- function(h)
  log((h - .2) / (1 - h))

## beta -> h
beta_to_h <- function(beta)
  (exp(beta) + .2) / (1 + exp(beta))

## dnorm for h
beta_to_h.dnorm <- function(h, mu, sigma) {
#  h <- (exp(beta) + .2) / (1 + exp(beta))
  beta <- log((h - .2) / (1 - h))
  (1/(h-.2) + 1/(1-h)) * 1 / sqrt(2*pi*sigma^2) * exp(-(beta - mu)^2 / (2*sigma^2))
}
